{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27256699",
   "metadata": {},
   "outputs": [],
   "source": [
    "%serialconnect --port=COM15"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2034e64e",
   "metadata": {},
   "outputs": [],
   "source": [
    "%rebootdevice"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e96d1af1",
   "metadata": {},
   "source": [
    "## 写入tools.py文件\n",
    "只需运行一次，写入到Flash中，掉电保存，非易失。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ff199ce5",
   "metadata": {},
   "outputs": [],
   "source": [
    "%sendtofile tools.py\n",
    "import struct\n",
    "import time\n",
    "\n",
    "class CANMotorController:\n",
    "    PARAMETERS = {\n",
    "        \"run_mode\": {\"index\": 0x7005, \"format\": \"u8\"},\n",
    "        \"iq_ref\": {\"index\": 0x7006, \"format\": \"f\"},\n",
    "        \"spd_ref\": {\"index\": 0x700A, \"format\": \"f\"},\n",
    "        \"limit_torque\": {\"index\": 0x700B, \"format\": \"f\"},\n",
    "        \"cur_kp\": {\"index\": 0x7010, \"format\": \"f\"},\n",
    "        \"cur_ki\": {\"index\": 0x7011, \"format\": \"f\"},\n",
    "        \"cur_filt_gain\": {\"index\": 0x7014, \"format\": \"f\"},\n",
    "        \"loc_ref\": {\"index\": 0x7016, \"format\": \"f\"},\n",
    "        \"limit_spd\": {\"index\": 0x7017, \"format\": \"f\"},\n",
    "        \"limit_cur\": {\"index\": 0x7018, \"format\": \"f\"},\n",
    "    }\n",
    "    TWO_BYTES_BITS = 16\n",
    "    def __init__(self, bus, motor_id=127, main_can_id=254):\n",
    "        self.bus = bus\n",
    "        self.MOTOR_ID = motor_id\n",
    "        self.MAIN_CAN_ID = main_can_id\n",
    "        self.P_MIN = -12.5\n",
    "        self.P_MAX = 12.5\n",
    "        self.V_MIN = -30.0\n",
    "        self.V_MAX = 30.0\n",
    "        self.KP_MIN, self.KP_MAX = 0.0, 500.0  # 0.0 ~ 500.0\n",
    "        self.KD_MIN, self.KD_MAX = 0.0, 5.0  # 0.0 ~ 5.0\n",
    "\n",
    "    # 通信类型\n",
    "    class CmdModes:\n",
    "        GET_DEVICE_ID = 0\n",
    "        MOTOR_CONTROL = 1\n",
    "        MOTOR_FEEDBACK = 2\n",
    "        MOTOR_ENABLE = 3\n",
    "        MOTOR_STOP = 4\n",
    "        SET_MECHANICAL_ZERO = 6\n",
    "        SET_MOTOR_CAN_ID = 7\n",
    "        SINGLE_PARAM_READ = 17\n",
    "        SINGLE_PARAM_WRITE = 18\n",
    "        FAULT_FEEDBACK = 21\n",
    "    def _float_to_uint(self, x, x_min, x_max, bits):\n",
    "        span = x_max - x_min\n",
    "        offset = x_min\n",
    "        x = max(min(x, x_max), x_min)  # Clamp x to the range [x_min, x_max]\n",
    "        return int(((x - offset) * ((1 << bits) - 1)) / span)\n",
    "    \n",
    "    def _uint_to_float(self, x, x_min, x_max, bits):\n",
    "        span = (1 << bits) - 1\n",
    "        offset = x_max - x_min\n",
    "        x = max(min(x, span), 0)  # Clamp x to the range [0, span]\n",
    "        return offset * x / span + x_min\n",
    "    \n",
    "    # 定义线性映射函数\n",
    "    def _linear_mapping(self, value, value_min, value_max, target_min=0, target_max=65535):\n",
    "        return int((value - value_min) / (value_max - value_min) * (target_max - target_min) + target_min)\n",
    "    \n",
    "    def format_data(self, data=[], format=\"f f\", type=\"decode\"):\n",
    "        format_list = format.split()\n",
    "        rdata = []\n",
    "        if type == \"decode\":\n",
    "            p = 0\n",
    "            for f in format_list:\n",
    "                s_f = []\n",
    "                if f == \"f\":\n",
    "                    s_f = [4, \"f\"]\n",
    "                elif f == \"u16\":\n",
    "                    s_f = [2, \"H\"]\n",
    "                elif f == \"s16\":\n",
    "                    s_f = [2, \"h\"]\n",
    "                elif f == \"u32\":\n",
    "                    s_f = [4, \"I\"]\n",
    "                elif f == \"s32\":\n",
    "                    s_f = [4, \"i\"]\n",
    "                elif f == \"u8\":\n",
    "                    s_f = [1, \"B\"]\n",
    "                elif f == \"s8\":\n",
    "                    s_f = [1, \"b\"]\n",
    "                ba = bytearray()\n",
    "                if len(s_f) == 2:\n",
    "                    for i in range(s_f[0]):\n",
    "                        ba.append(data[p])\n",
    "                        p = p + 1\n",
    "                    rdata.append(struct.unpack(s_f[1], ba)[0])\n",
    "                else:\n",
    "                    print(\"unknown format in format_data(): \" + f)\n",
    "                    return []\n",
    "            return rdata\n",
    "        elif type == \"encode\" and len(format_list) == len(data):\n",
    "            for i in range(len(format_list)):\n",
    "                f = format_list[i]\n",
    "                s_f = []\n",
    "                if f == \"f\":\n",
    "                    s_f = [4, \"f\"]\n",
    "                elif f == \"u16\":\n",
    "                    s_f = [2, \"H\"]\n",
    "                elif f == \"s16\":\n",
    "                    s_f = [2, \"h\"]\n",
    "                elif f == \"u32\":\n",
    "                    s_f = [4, \"I\"]\n",
    "                elif f == \"s32\":\n",
    "                    s_f = [4, \"i\"]\n",
    "                elif f == \"u8\":\n",
    "                    s_f = [1, \"B\"]\n",
    "                elif f == \"s8\":\n",
    "                    s_f = [1, \"b\"]\n",
    "                if f != \"f\":\n",
    "                    data[i] = int(data[i])\n",
    "                if len(s_f) == 2:\n",
    "                    bs = struct.pack(s_f[1], data[i])\n",
    "                    for j in range(s_f[0]):\n",
    "                        rdata.append(bs[j])\n",
    "                else:\n",
    "                    print(\"unkown format in format_data(): \" + f)\n",
    "                    return []\n",
    "            if len(rdata) < 4:\n",
    "                for i in range(4 - len(rdata)):\n",
    "                    rdata.append(0x00)\n",
    "            return rdata\n",
    "    \n",
    "    # 定义打包data1函数\n",
    "    def pack_to_8bytes(self, target_angle, target_velocity, Kp, Kd):\n",
    "        # 对输入变量进行线性映射\n",
    "        target_angle_mapped = self._linear_mapping(target_angle, self.P_MIN, self.P_MAX)\n",
    "        target_velocity_mapped = self._linear_mapping(target_velocity, self.V_MIN, self.V_MAX)\n",
    "        Kp_mapped = self._linear_mapping(Kp, self.KP_MIN, self.KP_MAX)\n",
    "        Kd_mapped = self._linear_mapping(Kd, self.KD_MIN, self.KD_MAX)\n",
    "        \n",
    "        # 使用Python的struct库进行打包\n",
    "        # 使用H表示无符号短整数（2字节），共需要8字节\n",
    "        data1_bytes = struct.pack('HHHH', target_angle_mapped, target_velocity_mapped, Kp_mapped, Kd_mapped)\n",
    "        data1 = [b for b in data1_bytes]\n",
    "        return data1\n",
    "    \n",
    "    def send_receive_can_message(self, cmd_mode, data2, data1, timeout=200):\n",
    "        \"\"\"\n",
    "        Send a CAN message and receive a response.\n",
    "        \n",
    "        Parameters:\n",
    "        - cmd_mode: Command mode (bit28~bit24)\n",
    "        - data2: Data area 2 (bit23~bit8)\n",
    "        - data1: Data bytes to send\n",
    "        - timeout: Timeout for sending the message (default is 200ms)\n",
    "        \n",
    "        Returns:\n",
    "        A tuple containing the received message data and the received message arbitration ID, if any.\n",
    "        \"\"\"\n",
    "        # Calculate the arbitration ID\n",
    "        arbitration_id = (cmd_mode << 24) | (data2 << 8) | self.MOTOR_ID\n",
    "\n",
    "        # Send the CAN message\n",
    "        try:\n",
    "            self.bus.send(data1, arbitration_id, timeout=timeout, extframe=True)\n",
    "        except:\n",
    "            print(\"Failed to send the message.\")\n",
    "            return None, None\n",
    "\n",
    "        # Output details of the sent message\n",
    "        print(f\"Sent message with ID {hex(arbitration_id)}, data: {data1}\")\n",
    "\n",
    "        # Receive a CAN message with a 1-second timeout\n",
    "        received_msg = self.bus.recv(list=None, timeout=1000)\n",
    "        if received_msg:\n",
    "            received_msg_arbitration_id, _, _, received_msg_data = received_msg\n",
    "            return received_msg_data, received_msg_arbitration_id\n",
    "        else:\n",
    "            return None, None\n",
    "    def parse_received_msg(self, data, arbitration_id):\n",
    "        if data is not None:\n",
    "            print(f\"Received message with ID {hex(arbitration_id)}\")\n",
    "            pos = self._uint_to_float(\n",
    "                (data[0] << 8) + data[1], self.P_MIN, self.P_MAX, self.TWO_BYTES_BITS\n",
    "            )\n",
    "            vel = self._uint_to_float(\n",
    "                (data[2] << 8) + data[3], self.V_MIN, self.V_MAX, self.TWO_BYTES_BITS\n",
    "            )\n",
    "            print(f\"pos:{pos:.2f} rad, vel:{vel:.2f} rad/s\")\n",
    "            return pos, vel\n",
    "        else:\n",
    "            print(\"No message received within the timeout period.\")\n",
    "            return None, None\n",
    "    def clear_can_rx(self, timeout=10):\n",
    "        \"\"\"\n",
    "        Clear the receiver buffer by reading all existing messages.\n",
    "        \n",
    "        Parameters:\n",
    "        - timeout: Time in ms to wait for the clearing operation.\n",
    "        \"\"\"\n",
    "        while self.bus.any():\n",
    "            recv_msg = self.bus.recv(list=None, timeout=timeout)\n",
    "            if recv_msg is not None:\n",
    "                print(f\"Cleared message with ID {hex(recv_msg[0])}\")\n",
    "        \n",
    "    def _write_single_param(self, index, value, format=\"u32\"):\n",
    "        encoded_data = self.format_data(data=[value], format=format, type=\"encode\")\n",
    "        data1 = [b for b in struct.pack(\"<I\", index)] + encoded_data\n",
    "\n",
    "        self.clear_can_rx()  # 空CAN接收缓存，避免读到老数据\n",
    "\n",
    "        received_msg_data, received_msg_arbitration_id = self.send_receive_can_message(\n",
    "            cmd_mode=self.CmdModes.SINGLE_PARAM_WRITE,\n",
    "            data2=self.MAIN_CAN_ID,\n",
    "            data1=data1,\n",
    "        )\n",
    "        self.parse_received_msg(received_msg_data, received_msg_arbitration_id)\n",
    "        return received_msg_data, received_msg_arbitration_id\n",
    "    def write_single_param(self, param_name, value):\n",
    "        param_info = self.PARAMETERS.get(param_name)\n",
    "        if param_info is None:\n",
    "            print(f\"Unknown parameter name: {param_name}\")\n",
    "            return\n",
    "\n",
    "        index = param_info[\"index\"]\n",
    "        format = param_info[\"format\"]\n",
    "\n",
    "        self._write_single_param(index=index, value=value, format=format)\n",
    "    def set_0_pos(self):\n",
    "        self.clear_can_rx()  # 清空CAN接收缓存，避免读到老数据\n",
    "\n",
    "        received_msg_data, received_msg_arbitration_id = self.send_receive_can_message(\n",
    "            cmd_mode=self.CmdModes.SET_MECHANICAL_ZERO,\n",
    "            data2=self.MAIN_CAN_ID,\n",
    "            data1=[1],\n",
    "        )\n",
    "        \n",
    "        self.parse_received_msg(received_msg_data, received_msg_arbitration_id)\n",
    "\n",
    "    def enable(self):\n",
    "        self.clear_can_rx(0)  # 清空CAN接收缓存，避免读到老数据\n",
    "\n",
    "        received_msg_data, received_msg_arbitration_id = self.send_receive_can_message(\n",
    "            cmd_mode=self.CmdModes.MOTOR_ENABLE,\n",
    "            data2=self.MAIN_CAN_ID,\n",
    "            data1=[]\n",
    "        )\n",
    "        self.parse_received_msg(received_msg_data, received_msg_arbitration_id)\n",
    "    \n",
    "    def disable(self):\n",
    "        self.clear_can_rx(0)  # 空CAN接收缓存，避免读到老数据\n",
    "\n",
    "        received_msg_data, received_msg_arbitration_id = self.send_receive_can_message(\n",
    "            cmd_mode=self.CmdModes.MOTOR_STOP,\n",
    "            data2=self.MAIN_CAN_ID,\n",
    "            data1=[0, 0, 0, 0, 0, 0, 0, 0]\n",
    "        )\n",
    "        self.parse_received_msg(received_msg_data, received_msg_arbitration_id)\n",
    "    def set_motor_position_control(self, limit_spd, loc_ref):\n",
    "        # 设置电机最大速度\n",
    "        self.write_single_param(param_name=\"limit_spd\" ,value=limit_spd)\n",
    "        # 设置电机目标位置\n",
    "        self.write_single_param(param_name=\"loc_ref\", value=loc_ref)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8e99a280",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tools import CANMotorController"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5f79af61",
   "metadata": {},
   "outputs": [],
   "source": [
    "%ls"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a940d97e",
   "metadata": {},
   "outputs": [],
   "source": [
    "%fetchfile tools.py --print"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5509e1f7",
   "metadata": {},
   "outputs": [],
   "source": [
    "%rebootdevice"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fed82c44",
   "metadata": {},
   "outputs": [],
   "source": [
    "%disconnect"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c0cfb767",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "MicroPython - USB",
   "language": "micropython",
   "name": "micropython"
  },
  "language_info": {
   "codemirror_mode": "python",
   "file_extension": ".py",
   "mimetype": "text/python",
   "name": "micropython"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
